#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <quadrotor_msgs/Corrections.h>
#include <quadrotor_msgs/PositionCommand.h>
#include <quadrotor_msgs/SO3Command.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <so3_control/SO3Control.h>
#include <std_msgs/Bool.h>
#include <tf/transform_datatypes.h>

//SO3话题管理
//输入话题:里程计数据，位置命令，速度命令，电机控制命令（可无），矫正数据？IMU数据
//输出话题：so3_cmd 包含了无人机飞行器所需要的力和目标姿态
struct PoseData {
  float last_position_x;
  float last_position_y;
  float last_position_z;
  ros::Time last_time;
};

PoseData last_pose_data;

class SO3ControlNodelet : public nodelet::Nodelet
{
public:
  SO3ControlNodelet()
    : position_cmd_updated_(false)//用于表示位置是否更新
    , position_cmd_init_(false)//判断位置命令是否初始化成功
    , des_yaw_(0)//目标偏航角
    , des_yaw_dot_(0)//目标偏航角
    , current_yaw_(0)//当前偏航角
    , enable_motors_(true)
    , // FIXME
    use_external_yaw_(true)//判断是否使用外部提供的偏航角数据
  {
  }

  void onInit(void);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private://定义私有成员，函数
  void publishSO3Command(void);//负责发送SO3命令到ROS中
  void position_cmd_callback(
    const quadrotor_msgs::PositionCommand::ConstPtr& cmd);
  void velocity_cmd_callback(
    const quadrotor_msgs::PositionCommand::ConstPtr& cmd);

  void odom_callback(const nav_msgs::Odometry::ConstPtr& odom);
  void enable_motors_callback(const std_msgs::Bool::ConstPtr& msg);
  void corrections_callback(const quadrotor_msgs::Corrections::ConstPtr& msg);
  void imu_callback(const sensor_msgs::Imu& imu);

  SO3Control      controller_;
  ros::Publisher  so3_command_pub_;
  ros::Subscriber odom_sub_;
  ros::Subscriber position_cmd_sub_;
  ros::Subscriber velocity_cmd_sub_;
  ros::Subscriber enable_motors_sub_;
  ros::Subscriber corrections_sub_;
  ros::Subscriber imu_sub_;

  bool        position_cmd_updated_, position_cmd_init_;
  std::string frame_id_;

  Eigen::Vector3d des_pos_, des_vel_, des_acc_, kx_, kv_;
  double          des_yaw_, des_yaw_dot_;
  double          current_yaw_;
  bool            enable_motors_;
  bool            use_external_yaw_;
  double          kR_[3], kOm_[3], corrections_[3];
  double          init_x_, init_y_, init_z_;
};

void
SO3ControlNodelet::publishSO3Command(void)//发送控制命令到ROS系统
{
  //计算控制力和姿态，传入目标位置，目标速度，目标加速度，目标yaw角，目标yaw角速度，以及控制增益
  controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_,
                               des_yaw_dot_, kx_, kv_);

  //把计算出来的控制力和姿态角度转移到变量中
  const Eigen::Vector3d&    force       = controller_.getComputedForce();
  const Eigen::Quaterniond& orientation = controller_.getComputedOrientation();

  quadrotor_msgs::SO3Command::Ptr so3_command(
    new quadrotor_msgs::SO3Command); //! @note memory leak?
  //包信息转移
  so3_command->header.stamp    = ros::Time::now();
  so3_command->header.frame_id = frame_id_;
  //目标三轴方向力
  so3_command->force.x         = force(0);
  so3_command->force.y         = force(1);
  so3_command->force.z         = force(2);
  //目标四元数姿态角度
  so3_command->orientation.x   = orientation.x();
  so3_command->orientation.y   = orientation.y();
  so3_command->orientation.z   = orientation.z();
  so3_command->orientation.w   = orientation.w();
  for (int i = 0; i < 3; i++)//设置控制增益
  {
    so3_command->kR[i]  = kR_[i];
    so3_command->kOm[i] = kOm_[i];
  }
  so3_command->current_yaw          = current_yaw_;
  so3_command->kf_correction        = corrections_[0];
  so3_command->angle_corrections[0] = corrections_[1];
  so3_command->angle_corrections[1] = corrections_[2];
  so3_command->enable_motors        = enable_motors_;
  so3_command->use_external_yaw     = use_external_yaw_;
  so3_command_pub_.publish(so3_command);//发布控制信息
}

//位置控制命令，把输入的目标位置赋给变量，控制数据完美符合egoplanner
void
SO3ControlNodelet::position_cmd_callback(
  const quadrotor_msgs::PositionCommand::ConstPtr& cmd)
{
  des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
  des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
  des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
                             cmd->acceleration.z);

  // if ( cmd->kx[0] > 1e-5 || cmd->kx[1] > 1e-5 || cmd->kx[2] > 1e-5 )
  // {
  //   kx_ = Eigen::Vector3d(cmd->kx[0], cmd->kx[1], cmd->kx[2]);
  // }
  // if ( cmd->kv[0] > 1e-5 || cmd->kv[1] > 1e-5 || cmd->kv[2] > 1e-5 )
  // {
  //   kv_ = Eigen::Vector3d(cmd->kv[0], cmd->kv[1], cmd->kv[2]);
  // }

  des_yaw_              = cmd->yaw;
  des_yaw_dot_          = cmd->yaw_dot;
  position_cmd_updated_ = true;
  position_cmd_init_    = true;

  publishSO3Command();//数据赋予后开始执行控制计算并且输出控制数据
}

//速度控制命令（貌似代码被修改了，目前是速度控制和位置控制结合，大疆官方数据不会回传当前速度值，只能用位置控制）
void
SO3ControlNodelet::velocity_cmd_callback(
  const quadrotor_msgs::PositionCommand::ConstPtr& cmd)
{
  des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
  des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
  des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
                             cmd->acceleration.z);

  // if ( cmd->kx[0] > 1e-5 || cmd->kx[1] > 1e-5 || cmd->kx[2] > 1e-5 )
  // {
  //   kx_ = Eigen::Vector3d(cmd->kx[0], cmd->kx[1], cmd->kx[2]);
  // }
  // if ( cmd->kv[0] > 1e-5 || cmd->kv[1] > 1e-5 || cmd->kv[2] > 1e-5 )
  // {
  //   kv_ = Eigen::Vector3d(cmd->kv[0], cmd->kv[1], cmd->kv[2]);
  // }

  des_yaw_              = cmd->yaw;
  des_yaw_dot_          = cmd->yaw_dot;
  position_cmd_updated_ = true;
  position_cmd_init_    = true;

  publishSO3Command();
}

// void
// SO3ControlNodelet::keyboard_velocity_cmd_callback(
//   const geometry_msgs::Twist::ConstPtr& cmd)
// {
//   des_vel_ = Eigen::Vector3d(cmd->linear.x, cmd->linear.y, cmd->linear.z);

//   des_yaw_              = cmd->angular.z;
//   position_cmd_updated_ = true;
//   position_cmd_init_    = true;

//   publishSO3Command();//数据赋予后开始执行控制计算并且输出控制数据
// }

//里程计回调函数
//雷达回传的数据只有位置和方向的四元数
void
SO3ControlNodelet::odom_callback(const geometry_msgs::PoseStamped::ConstPtr& odom)
{
  const Eigen::Vector3d position(odom->pose.position.x,
                                 odom->pose.position.y,
                                 odom->pose.position.z);
  
  //记下本次时间戳
  ros::Time current_time = odom->header.stamp;
  //计算两次时间差
  ros::Duration time_diff = current_time - last_pose_data.last_time;

  //计算无人机三轴速度
  float velocity_x = (odom->pose.position.x - last_pose_data.last_position_x)/time_diff;
  float velocity_y = (odom->pose.position.y - last_pose_data.last_position_y)/time_diff;
  float velocity_z = (odom->pose.position.z - last_pose_data.last_position_z)/time_diff;

  //存入向量
  const Eigen::Vector3d velocity(velocity_x,
                                 velocity_y,
                                 velocity_z);

  current_yaw_ = tf::getYaw(odom->orientation);//通过四元数得到当前的yaw角


  controller_.setVelocity(velocity);
  controller_.setPosition(position);


  if (position_cmd_init_)
  {
    if (!position_cmd_updated_)
      publishSO3Command();
    position_cmd_updated_ = false;
  }
  else if ( init_z_ > -9999.0 )
  {
    des_pos_ = Eigen::Vector3d(init_x_, init_y_, init_z_);
    des_vel_ = Eigen::Vector3d(0,0,0);
    des_acc_ = Eigen::Vector3d(0,0,0);
    publishSO3Command();
  }
  //更新上次时间戳
  last_pose_data.last_time = current_time;
  last_pose_data.last_position_x = odom->pose.position.x;
  last_pose_data.last_position_y = odom->pose.position.y;
  last_pose_data.last_position_z = odom->pose.position.z;
}

//使能电机（可能不需要）
void
SO3ControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr& msg)
{

  enable_motors_ = 1;
}

void
SO3ControlNodelet::corrections_callback(
  const quadrotor_msgs::Corrections::ConstPtr& msg)
{
  corrections_[0] = msg->kf_correction;
  corrections_[1] = msg->angle_corrections[0];
  corrections_[2] = msg->angle_corrections[1];
}

void
SO3ControlNodelet::imu_callback(const sensor_msgs::Imu& imu)
{
  const Eigen::Vector3d acc(imu.linear_acceleration.x,
                            imu.linear_acceleration.y,
                            imu.linear_acceleration.z);
  controller_.setAcc(acc);
}

void
SO3ControlNodelet::onInit(void)
{
  ros::NodeHandle n(getPrivateNodeHandle());

  std::string quadrotor_name;
  n.param("quadrotor_name", quadrotor_name, std::string("quadrotor"));
  frame_id_ = "/" + quadrotor_name;

  double mass;
  n.param("mass", mass, 0.9);//存储无人机质量，rmua为0.9KG
  controller_.setMass(mass);

  n.param("use_external_yaw", use_external_yaw_, true);

  //定义一些初始化数值，如果没有接收到对应数据则使用该数值
  n.param("gains/rot/x", kR_[0], 1.5);
  n.param("gains/rot/y", kR_[1], 1.5);
  n.param("gains/rot/z", kR_[2], 1.0);
  n.param("gains/ang/x", kOm_[0], 0.13);
  n.param("gains/ang/y", kOm_[1], 0.13);
  n.param("gains/ang/z", kOm_[2], 0.1);
  n.param("gains/kx/x", kx_[0], 5.7);
  n.param("gains/kx/y", kx_[1], 5.7);
  n.param("gains/kx/z", kx_[2], 6.2);
  n.param("gains/kv/x", kv_[0], 3.4);
  n.param("gains/kv/y", kv_[1], 3.4);
  n.param("gains/kv/z", kv_[2], 4.0);

  n.param("corrections/z", corrections_[0], 0.0);
  n.param("corrections/r", corrections_[1], 0.0);
  n.param("corrections/p", corrections_[2], 0.0);

  //初始化xyz值，并非无人机初始位置，只是一个占位数值，不需要改
  n.param("so3_control/init_state_x", init_x_, 0.0);
  n.param("so3_control/init_state_y", init_y_, 0.0);
  n.param("so3_control/init_state_z", init_z_, -10000.0);

  //定义SO3命令话题发布者
  so3_command_pub_ = n.advertise<quadrotor_msgs::SO3Command>("so3_cmd", 10);

  //订阅坐标话题，目前测试先用状态真值
  odom_sub_ = n.subscribe("/airsim_node/drone_1/debug/pose_gt", 10, &SO3ControlNodelet::odom_callback, this,
                          ros::TransportHints().tcpNoDelay());
  //订阅位置命令话题
  position_cmd_sub_ =
    n.subscribe("position_cmd", 10, &SO3ControlNodelet::position_cmd_callback,
                this, ros::TransportHints().tcpNoDelay());
  //订阅速度命令话题
  velocity_cmd_sub_ =
    n.subscribe("velocity_cmd", 10, &SO3ControlNodelet::velocity_cmd_callback,
                this, ros::TransportHints().tcpNoDelay());
  //订阅电机话题？
  enable_motors_sub_ =
    n.subscribe("motors", 2, &SO3ControlNodelet::enable_motors_callback, this,
                ros::TransportHints().tcpNoDelay());
  //订阅校正参数
  corrections_sub_ =
    n.subscribe("corrections", 10, &SO3ControlNodelet::corrections_callback,
                this, ros::TransportHints().tcpNoDelay());

  //订阅IMU话题
  imu_sub_ = n.subscribe("airsim_node/drone_1/imu/imu", 10, &SO3ControlNodelet::imu_callback, this,
                         ros::TransportHints().tcpNoDelay());

  //还需要再订阅键盘控制命令话题，写回调函数设定键盘控制无人机飞行
}

#include <pluginlib/class_list_macros.h>
//PLUGINLIB_DECLARE_CLASS(so3_control, SO3ControlNodelet, SO3ControlNodelet,
//                        nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(SO3ControlNodelet, nodelet::Nodelet);
